 
// Dancing Shoes Software
// Draft 0.1 - 5th of December

#include "servofunc.h"

#define UP		0
#define DOWN 	1
#define SERVOS 	4
#define EVENTS 	10
#define LIGHTS	4

#define L1		0x02
#define L2		0x04
#define L3		0x06
#define L4		0x08

// Some useful typedef's
typedef unsigned char uint8;
typedef unsigned int uint16;
typedef unsigned long uint32;

// Names of motions / events
typedef enum {
	NA,

	CL1, 
	CL2, 
	CL3,

	CR1, 
	CR2,
	CR3, 

	LC1, 
	LC2, 
	LC3, 

	RC1, 
	RC2, 
	RC3,

	DELAY

} event_name_t;

// Event status
typedef enum {	
	NOT_LOADED, WAITING, RUNNING, FINISHED
} event_status_t;

// Event 
typedef struct {
	// Unique name of the event
	event_name_t name;
	// The duration of the event
	uint16 duration;
} event_t;


// Motion
typedef struct {
	// Unique name
	event_name_t name;
	// Begin and end positions	
	uint16 begin, end;
} motion_def_t;

// Data used when an event / motion
// is running
typedef struct {
	// References to motion and event definitions
	motion_def_t * def;
	event_t * event;
	// Index in the event table
	uint8 index;
	// Status of execution
	event_status_t status;
	// Direction of motion
	uint8 dir;
	// Position, distance, and number of ticks
	// per position step
	uint16 pos, target, dist, each;
} motion_data_t;

typedef struct {
	uint8 device, r1, g1, b1, r2, g2, b2;
	uint16 duration;
} light_event_t;

typedef struct {
	light_event_t * ref;

	uint8 index;
	uint8 r, g, b;
	uint8 r_e, g_e, b_e, r_dir, g_dir, b_dir;

	event_status_t status;

} light_event_meta_t;


// --- Tables ----

// Motion definitions
code motion_def_t definitions[] = {
	{CR1, 	500, 	1000},
	{CR2,	500,	700},
	{RC1, 	1000, 	500},
	{RC2,	700,	500},
	{LC1, 	0, 		500},
	{LC2,	300,	500},
	{CL1, 	500, 	0},
	{CL2,	500, 	300}
};

// Event sequence definitions
// - two-dimentionel array, one row for each servo
code event_t events[SERVOS][EVENTS] = {
	{
		{CR1, 1500},
		{RC1, 400},
		{0},
		{0},
		{0}, 
		{0}, 
		{0}, 
		{0},   
		{0}, 										  
		{0}
	},
	{
		{CR2, 1700},
		{RC2, 1700},
		{DELAY, 5000},
		{CL2, 1700}, 
		{LC2, 1700}, 
		{0},
		{0}, 
		{0}, 
		{0},
		{0}
	}
};

code light_event_t light_events[] = {
	{0, 0, 0, 0, 255, 100, 0, 2000},
	{0, 255, 100, 0, 255, 255, 0, 1000},
	{0, 255, 255, 0, 0, 0, 0, 1000},

	{1, 0, 0, 0, 255, 255, 255, 1500},
	{1, 255, 255, 255, 0, 0, 0, 1500}
};

// Global book keeping variables
uint16 ticks, cnt;
uint8 i, j;
// DEBUG
uint16 w1, w2, w3;

// Data about the running events
idata motion_data_t md[SERVOS];

idata light_event_meta_t ld[LIGHTS];

uint8 event_count(uint8);
void event_find_next(motion_data_t *, uint8);
void event_load_motion_def(motion_data_t *);
void event_initialize(motion_data_t *);
void event_update(motion_data_t *);

void light_update_all();
bit light_has_events(uint8);
void light_find_next_event(uint8, light_event_meta_t *);
void light_prepare_event(light_event_meta_t *);

void main()
{	
	motion_data_t * current;	   

	init_PCA();

	while (1)
	{
		//w1 = md[0].pos;
		//w2 = md[1].pos;

		light_update_all();
						  /*
		// go through all the motion metas
		for (i = 0; i < SERVOS; i++)
		{
			current = &md[i];			

			// load event
			if (current->event == 0) // status == NOT_LOADED or FINISHED
			{
				// find next event
				event_find_next(current, i);

	 			// load motion definition
				event_load_motion_def(current);

				// if no definition could be found, ignore this event
				if (current->status != WAITING)	// status != WAITING <=> def = NULL
				{
					current->event = 0;
				}
			}

			// make it ready...
			if (current->status == WAITING)
			{
				event_initialize(current);
			}

			// run run run
			if (current->status == RUNNING)
			{
				if (ticks % current->each == 0)
				{
					event_update(current);
				}
			}

		   	if (current->event->name != DELAY)
			{
				set_servo(i + 1, current->pos);
			}
		}
		*/
		for (cnt = 0; cnt < 10000; cnt++);

		ticks++;	
	}
}

void event_find_next(motion_data_t * d, uint8 x)
{
	d->event = &events[x][d->index];
				
	// solve next index
	d->index++;
	if (d->index == event_count(x))
	{
		d->index = 0;
	}
}

void event_load_motion_def(motion_data_t * d)
{
	uint8 x;
	
	if (d->event->name == DELAY)
	{
		d->def = 0;
		d->status = WAITING;

		return;
	}

	// find def 
	for (x = 0; x < ( sizeof(definitions)/sizeof(motion_def_t) ); x++)
	{
		if (d->event->name == definitions[x].name)
		{
			d->def = &definitions[x];

			// ready to go
			d->status = WAITING;
		}
	}
}

void event_initialize(motion_data_t * d)
{
	if (d->event->name == DELAY)
	{
		d->pos = 0;
		d->target = d->event->duration;

		d->dir = UP;
		d->each = 1;
		d->status = RUNNING;

		return;
	}

	d->target = d->def->end;

	// direction
	d->dir = (d->def->begin < d->def->end) ? UP : DOWN;

	// distance
	d->dist = (d->dir == UP) ? d->def->end - d->def->begin 
		: d->def->begin - d->def->end;

	// steps
	d->each = d->event->duration / d->dist;

	// position
	d->pos = d->def->begin;

	d->status = RUNNING;
}

void event_update(motion_data_t * d)
{
  	d->pos += (d->dir == UP) ? 10 : -10;

	if (d->pos == d->target)
	{
		d->status = FINISHED;
		d->event = 0;
	}
}

// Returns the number of events defined 
// for each servo
uint8 event_count(uint8 index)
{
	uint8 count, len = 0;
	for (count = 0; count < EVENTS; count++)
	{
		if (events[index][count].name != NA)
		{	
			len++;
		}
	}
	return len;
}

void light_update_all()
{
	light_event_meta_t * current;
	
	w1 = ld[0].r;
	w2 = ld[1].r;

	for (i = 0; i < LIGHTS; i++)
	{
		current = &ld[i];

		// no event loaded
		if (current->ref == 0)
		{
			light_find_next_event(i, current);	
		}

		// event loaded - prepare for running
		if (current->status == WAITING)
		{
			light_prepare_event(current);
		}

		// run
		if (current->status == RUNNING)
		{
			if (ticks % current->r_e == 0)
			{
				if (current->r != current->ref->r2)
				{
					current->r += (current->r_dir == UP) ? 1 : -1;
				}
			}
			if (ticks % current->g_e == 0)
			{
				if (current->g != current->ref->g2)
				{
					current->g += (current->g_dir == UP) ? 1 : -1;
				}
			}
			if (ticks % current->b_e == 0)
			{
				if (current->b != current->ref->b2)
				{
					current->b += (current->b_dir == UP) ? 1 : -1;
				}
			}

			if (current->r == current->ref->r2 && 
				current->g == current->ref->g2 && 
				current->b == current->ref->b2)
			{
				current->ref = 0;
				current->status = FINISHED;
			}
		}
	}
}

void light_find_next_event(uint8 device, light_event_meta_t * m)
{
	uint8 x = m->index, length = sizeof(light_events) / sizeof(light_event_t);
	light_event_t * ptr;
	
	if (light_has_events(device) == 0)
	{
		m->ref = 0;
		return;
	}

	while (1)	
	{
		ptr = &light_events[x];
		
		if (ptr->device == device)
		{
			m->ref = ptr;
			m->index = (x == (length - 1)) ? 0 : x + 1;
			m->status = WAITING;

			return;
		}

		x++;
		if (x == length)
		{
			x = 0;
		}
	}
}

bit light_has_events(uint8 device)
{
	uint8 x,  length = sizeof(light_events) / sizeof(light_event_t);

	for (x = 0; x < length; x++)
	{
		if (light_events[x].device == device)
		{
			return 1;
		}
	}
	return 0;
}

void light_prepare_event(light_event_meta_t * m)
{
	uint8 tmp;

	m->r = m->ref->r1, m->g = m->ref->g1, m->b = m->ref->b1;

	m->r_dir = (m->ref->r1 < m->ref->r2) ? UP : DOWN;
	m->g_dir = (m->ref->g1 < m->ref->g2) ? UP : DOWN;
	m->b_dir = (m->ref->b1 < m->ref->b2) ? UP : DOWN;

	tmp = (m->r_dir == UP) ? m->ref->r2 - m->ref->r1 : m->ref->r1 - m->ref->r2;
	m->r_e = m->ref->duration / tmp;

	tmp = (m->g_dir == UP) ? m->ref->g2 - m->ref->g1 : m->ref->g1 - m->ref->g2;
	m->g_e = m->ref->duration / tmp;

	tmp = (m->b_dir == UP) ? m->ref->b2 - m->ref->b1 : m->ref->b1 - m->ref->b2;
	m->b_e = m->ref->duration / tmp;

	m->status = RUNNING;
}
